#include "robot_one_navigation/nav_planner_test.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_core/exceptions.hpp"

namespace nav_planner_test
{
    void NavPlannerTest::configure(
        const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
        std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
        std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
    {
        buffer_ = tf;
        node_ = parent.lock();
        name_ = name;
        costmap_ = costmap_ros->getCostmap();
        global_frame_ = costmap_ros->getGlobalFrameID();

        nav2_util::declare_parameter_if_not_declared(
            node_, name_ + ".interpolation_resolution", rclcpp::ParameterValue(0.1));
        node_->get_parameter(name_ + ".interpolation_resolution", interpolation_resolution_);
    }

    void NavPlannerTest::cleanup()
    {
        RCLCPP_INFO(node_->get_logger(), "正在清除类型为NavPlannerTest的插件 %s", name_.c_str());
    }

    void NavPlannerTest::activate()
    {
        RCLCPP_INFO(node_->get_logger(), "正在激活类型为NavPlannerTest的插件 %s", name_.c_str());
    }

    void NavPlannerTest::deactivate()
    {
        RCLCPP_INFO(node_->get_logger(), "正在停用类型为NavPlannerTest的插件 %s", name_.c_str());
    }

    nav_msgs::msg::Path NavPlannerTest::createPlan(
        const geometry_msgs::msg::PoseStamped &start,
        const geometry_msgs::msg::PoseStamped &goal)
    {
        // 初始化路径
        nav_msgs::msg::Path global_path;
        global_path.header.frame_id = global_frame_;
        global_path.header.stamp = node_->now();
        global_path.poses.clear();
        // 起始点和终点的有效性判断
        if (start.header.frame_id != global_frame_ || goal.header.frame_id != global_frame_)
        {
            RCLCPP_INFO(node_->get_logger(), "规划器接受来自 %s 坐标系的起始位置和终点位置！", global_frame_.c_str());
            return global_path;
        }
        double path_length = std::hypot(goal.pose.position.x - start.pose.position.x, goal.pose.position.y - start.pose.position.y);
        double path_point = path_length / interpolation_resolution_;
        double x_interpolation_resolution = (goal.pose.position.x - start.pose.position.x) / path_point;
        double y_interpolation_resolution = (goal.pose.position.y - start.pose.position.y) / path_point;
        // 生成路径
        for (int i = 0; i < static_cast<int>(path_point); ++i)
        {
            geometry_msgs::msg::PoseStamped poses;
            poses.header.frame_id = global_frame_;
            poses.header.stamp = node_->now();
            poses.pose.position.x = start.pose.position.x + x_interpolation_resolution * i;
            poses.pose.position.y = start.pose.position.y + y_interpolation_resolution * i;
            global_path.poses.emplace_back(poses);
        }
        // 检查路径是否经过障碍物
        for (geometry_msgs::msg::PoseStamped poses : global_path.poses)
        {
            unsigned int mx, my;
            if (costmap_->worldToMap(poses.pose.position.x, poses.pose.position.y, mx, my))
            {
                RCLCPP_INFO(node_->get_logger(), "world坐标系转换到map坐标系成功！");
                unsigned char cost = costmap_->getCost(mx, my);
                if (cost == nav2_costmap_2d::LETHAL_OBSTACLE)
                {
                    RCLCPP_WARN(node_->get_logger(), "在x:%.2f,y:%.2f处为障碍物，规划失败！", poses.pose.position.x, poses.pose.position.y);
                    throw nav2_core::PlannerException("无法创建目标规划：" +
                                                      std::to_string(goal.pose.position.x) +
                                                      "," + std::to_string(goal.pose.position.y));
                }
            }
        }
        // 最后一个点
        geometry_msgs::msg::PoseStamped goal_pose = goal;
        goal_pose.header.frame_id = global_frame_;
        goal_pose.header.stamp = node_->now();
        global_path.poses.emplace_back(goal_pose);
        return global_path;
    }

} // namespace nav_planner_test

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav_planner_test::NavPlannerTest, nav2_core::GlobalPlanner)